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(57) Abstract: One embodiment the invention comprises a method of determining an orientation of a sensor. The method includes 
measuring a local magnetic field vector and a local gravity vector and using those measurements to determine the orientation of 
the sensor. Embodiments can include measuring the magnetic field vector and the local gravity vector using quaternion coordinates. 
Another embodiment comprises measuring a local magnetic field vector, a local gravity vector, and the angular velocity of the sensor. 
These three vectors are processed to determine the orientation of the sensor. In one embodiment the three vectors can all be measured 
in quaternion coordinates. Another method embodiment comprises determining a local gravity vector by providing a acceleration 
detector, moving the detector from a start point to an end point over a time period, and summing acceleration measurements over 
the time period. The local gravity vector is calculated using the summed acceleration measurements. A system embodiment of 
the present invention includes a body having mounted thereon at least one sensor. The at least one sensor is configured to output 
orientation information to at least one processing unit that inputs the orientation information into a synthetic environment. The 
system also can include a display for displaying the orientation of the body with respect to the synthetic environment. 
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Method and Apparatus for Motion Tracking of an 
Articulated Rigid body 

5 

related application 

This application is related to, and claims priority from theU.S, Provisional 
Application Serial No. 60/246,215, entitled "Apparatus For Real Time Tracking And 
10 Display Of The Position And Posture Of A Body Using Hybrid Sourceless Sensors, 
RF Positioning, And A Quaternion Based Complementary Filtering Algorithm", filed 
on October 30, 2000. This provisional application is hereby incorporated by reference 
in its entirety. 

15 technical field 

The invention described herein relates to methods and s^paratus for tracking 
the orientation (also referred to herein as posture) of an object More particularly, the 
invention relates to methods and apparatus for tracking the posture of an articulated 
rigid body. Still more particularly, the invention relates to methods and apparatus for 
20 tracking the posture of articulated rigid bodies using quaternion based attitude 
estimation filtering and displaying the posture of the body. 

BACKGROUND 
Snbodiments of the present invention are directed towards methods and 
25 devices for tracking the orientation (posture) of hnman bodies. In particular such 
orientation information can be inserted into a synthetic (computer generated) 
environmmt where the motion of the tracked body can become part of the synthetic 
environment. Previous motion tracking systems of the type known in flie art have a 
number of hmitations that substantially Umit their usefiihiess. 
30 Currently available motion tracking technologies are limited by then: reliance 

on a generated signal and/or the need to have tiie tracked body remam in sight of fnced 
stations positioned around a working volimie. In either case there is a requirement to 
maintain some type of link over a distance. Regardless of the type of signal used, it 

1 
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can be generally referred to as a "source." Usually, the effective range over which the 
link may be maintained is limited. Moreover, data update rates may be limited by the 
physical characteristics of the source used. Additionally, interference with, or 
distortion of; the source can result in erroneous orientation measurements. If the link 

5 is broken, a complete loss of track will result. 

One type of tracking system known in the art is the so-called mechanical 
tracking system. Such systems use an artificial exo-skeleton, which is wom by the 
user of a synthetic environment (typically, a computer-created simulated 
environment). Sensors (e.g., goniometers) within the skeletal linkages of the exo- 

10 skeleton have a general correspondence to the actual joints of the user. Joint angle 
data is fed into kinematic algorithms that are used to determine body posture and limb 
position. However, since the exo-skeleton is wom by the user, other systems must be 
used to ascertain the position of the user within the simulated environment. Such 
systems are fraugjit with numerous drawbacks. For one, aligning the goniometers 

15 with the joints of a human body is difficult, especially with multiple degree of 

freedom (DOF) joints. Additionally, the joints of the exo-skeleton cannot perfectly 
replicate ttie range of motion of the joints of a human body. Thus, such technologies 
can provide only a rough approximation of actual body movement. Another 
limitation stmis from the fact that human bodies are of different sizes and dhnensions. 

20 As a result, the exo-skeleton must be recalibrated for each user. Yet another 

limitation is imposed by the encumbrance of the exo-skeleton itself The weight and 
awkward configuration of the exo-skeleton prevent a human user from interacting 
with his environment in a natural manner. As a result, it is unlikely that the user will 
become immersed in the synthetic environment in the desired manner. 

25 Another widely xised system is a magnetic tracking system. In such systems a 

large magnetic field is g^erated and calibrated. The user has many small sensors 
mounted at various points on his body. The sensors are sensitive to the generated 
magnetic field. Thus, changes in position and orientation of the users body with 
respect to the generated magnetic field can be detected by the magnetic sensors. 

30 Some of drawbacks of such systems include very short range and difficulty in 
calibrating the generated magnetic field. The short range stems 6om the fact that 
magnetic fields decrease in power inversely with the square of the distance from the 
generating source. This restricts the use of such systems to areas about the size of a 
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small room. Jn order to use a larger working area, user movement must be modiJBed 
or scaled in some manner. As a result, the magnitude and frequency of position and 
orientation errors increase rs^idly. Additionally, the presence of ferromagnetic 
material (Uke the metal in belt buckles or weapons) distorts the generated magnetic 

5 fields. Additionally, the magnetic sensors pick up noise from other magnetic fields 
generated in or near the environment. Unfortunately, these distorting magnetic fields 
are commonplace, being easily generated by a plethora of devices, including 
computer monitors, fluorescent lighting, powered electrical wiring in the walls, as 
well as many other sources. Additionally, other sources of magnetic field error exist 

10 Only with the aid of extremely detailed look-\q) tables can even mod^ately accurate 
measurements be obtained. Thus, magnetic tracking based on a generated magnetic 
field is subject to positional and orimtation inaccuracies which are highly variable 
and unpredictable. 

Another system for detecting position and orientation of a body uses so-called 

15 optical sensing. Optical srasing, in general, covers a large and varying collection of 
technologies. All of these technologies depend on the sensing of some type of light to 
provide position and orientation information. Consequently, all of these technologies 
are subject to inaccuracies whenever a required light path is blocked. Additionally, 
these technologies suffer from interference from other light sources. All of these 

20 optical sensing systems require specially prepared environments having the necessary 
emitters and sensors. This prevents widespread usage and presents a significant and 
expulsive limitatioiL 

Yet another approach is a tracking system using acoustic trackers. Like the 
previously described magnetic trackers, such systems are limited in range due to the 

25 inherent limitations of sound propagation. Additionally, the physics of sound limit 
accuracy, information vpdsto rale, and the overall range of an acoustic tracking 
system. Moreover, due to the relatively directional nature of sound, clear lines of 
sight must be maintained in order to obtain accurate readings. Additionally, due to 
the relatively slow speed of sound, there are latency problems with such acoustic 

30 sensor systems. As is evident from the foregoing discussion, conventional approaches 
for the tracking of body orientation have some serious limitations. 
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SUMMARY OF THE INVENTION 
In accordance with the principles of the present invention, systems, method, 
and apparatus for body tracking is disclosed. A method embodiment for tracking the 
orientation of a sensor comprises measuring an angular velocity of the sensor to 

5 generate angular rate values which are integrating and normalizing to produce an 
estimate of sensor orientation. The method continues by measuring a local magnetic 
field vector and measuring local gravity vector and correcting the estimate of sensor 
orientation using the local magnetic field vector and local gravity vector. 

Another method embodiment comprises measuring an angular velocity of the 

10 sensor to generate an angular rate quaternion, integrating and normalizing the angular 
rate quaternion to produce an estimated sensor orientation quaternion. Hie local 
magnetic field vector and local gravity vector are measured. A measurement vector is 
determined fi:om the local magnetic field and local gravity vectors. A computed 
measurement vector is calculated firom the estimated sensor orientation quaternion 

15 and comparing with the measurement vector to generate an error vector lhat defines a 
criterion fimction. The criterion fimction is minimized and an error estimate 
quaternion is ou^ut. The error estimate quaternion is integrated and normalized to 
produce a new estimated sensor orientation quaternion which can be output as a 
sensor orientation signal. The entire process is repeated except that the new estimated 

20 sensor orientation quaternion is xised for calculating a computed measuremmt vector. 
The process continues until tracking is no longer desired. 

Another method embodiment of tracking the orientation of a sensor comprises 
providing a starting estimate of sensor orientation, measuring the local magnetic field 
vector and measuring the local gravity vector. A measurement vector is determined 

25 from the local magnetic field vector and the local gravity vector. A computed 
measurement vector is calculated firom the estimate of sensor orientation and 
compared to the measurement vector to generate an error vector that dejBnes a 
criterion fimction. A Gauss-Newton iteration is performed, resulting in a minimized 
criterion fimction generating an error estimate that is integrated and normalized to 

30 produce a new estimate of sensor orientation. As with the forgoing embodiment, the 
entire process is repeated except that the new estimate of sensor orientation is used for 
calculating a computed measurement vector. The process continues until tracking is 
no longer desired. 



4 



wo 02/37827 PCT/USO 1/46226 

Yet another method of tracking the orientation of a sensor comprises 
providing a starting estimate of sensor orientation quaternion measuring a local 
magnetic field vector and a local gravity vector and determining a measurement 
vector fix)m the local magnetic field vector and the local gravity vector. A computed 

5 measurement vector is calculated firom the estimate of sensor orientation, using 
quaternion mathematics. 

The measurement vector is compared with the computed measurement vector 
to generate an 6x1 error vector that defines a criterion Amotion and a mathematical 
operation is performed that results in the minimization of the criterion function and 

10 outputs a 4x1 quaternion error estimate. The quaternion error estimate is integrated 
and normalized to produce a new estimated sensor orientation quaternion* The entire 
process is repeated except that the new estimated sensor orientation quaternion is used 
for calculating a computed measurement vector. The process continues until tracking 
is no longer desired. 

15 A method for determining a local gravity vector values comprises, moving the 

sensor from a start point to an end point over a time period; taking measurements of 
the total acceleration vector during the time period, time weighted summing the 
measurements of the acceleration vector over the time period, and calculating gravity 
vector values using the summed total acceleration measurements. 

20 Embodiments of the invention also include sensor q)paratus comprising a 

magnetic field detector configured to measure a magnetic field vector and ou^ut a 
local magnetic field vector signal and an acceleration detector configured to detect a 
local gravitational field vector and output a local gravitational field vector signal. A 
related embodiment further includes an angular velocity detector configured to detect 

25 an angular velocity vector of the sensor and output angular velocity signal. 

Embodiments of the invention include a system for tracking the posture and 
orientation of body, the system comprising a body having mounted thereon at least 
one sensor, each sensor including a magnetometer for measuring a magnetic field 
vector and a acceleration detector for measuring a body acceleration vector, and at 

30 least one processor for receiving input firom the magnetometer and acceleration 

detector and xising said input to calculate a local magnetic field vector and a local 

gravity vector and to determine the orientation of the body. The processor of the 

system can be further configured to correct for offset between body coordinates and 

sensor coordinates. The system can further include a display for displaying the 

5 
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position and orientation of the body with respect to a synthetic environment. Further 
embodiments use sensors that include angular velocity detectors. 

Other aspects and advantages of the invention will become apparent from the 
following detailed description and accompanying drawings which illustrate, by way 
5 of example, the principles of the invention. 

BRIEF DESCRIPTION OF THE DRAWINGS 

The following detailed description of the embodiments of the invention will be 
more readily understood in conjunction with the accompanying drawings, in which: 

10 Fig. 1 is a figurative depiction of the earth's surface and its relationship with a 

example pair of a magnetic field vector and gravity vector. 

Figs, 2(a) and 2(b) depict reference and body coordinate systems respectively 

Fig. 3 is a simplified block diagram of a filtering metiiod embodiment in 
accordance with the principles of the present invention 

15 Fig. 4 is a block diagram of a system embodiment of the present invention. 

Figs. 5 and 6 are flow diagrams illustratmg method embodiments in 
accordance with the principles of the present inventioa 

It is to be understood that in the drawings like reference numerals designate 
like structural elements. 

20 

DETAILED DESCRIPTION OF THE DRAWINGS 

The embodiments of the present invention provide a method and apparatus for 
tracking the posture of a body without the need for a generated field (or source) or a 
plurality of fixed stations. Advances in the field of miniature sensors over the last 

25 decade make possible inertial/magnetic tracking of the orientation of tracked body in 
three dimensions. In particular such sensors can be used to track human body limb 
segments in three dimensions. In accordance with the principles of the present 
invention such tracking incorporates the passive measurement of physical quantities 
that are directly related to the rate of rotation and orientation of a rigid body. The 

30 "sourceless" nature of this technique makes possible fiill body posture tracking of 
multiple users over an area that is only limited by the range of a wireless LAN. Since 
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orientation estimates are based only on passive measurements, nearly all latency in 
such a system is due to the computational demands of the data processing algorithms 
involved and not physical characteristics of the generated source. 

An embodiment of the present invention makes use of the fact that a local 

5 magnetic field vector and local gravity vector can be defined. Figure 1 is a simplified 
figurative illustration of the surface of the earth 10 showing a local gravity vectors 1 1 
and a local magnetic field vector 13. In general, for any object positioned on the earth 
10, the gravity vectors always points to the earth's center of mass, which may be 
defined as "down". This phenomenon is simply illustrated in Figure 1. Additionally, 

10 the magnetic field vector 1 3 always points to magnetic north. 

Sensor embodiments of the present invention, by tracking changes in the 
orientation of the sensor with respect to the local magnetic field vector and the local 
gravity vector, can detect changes m orientation of the sensor. An appropriately 
designed sensor can track the orientation of a body. Significantly, a system having a 

15 plurality of sensors, each mounted to a limb of an articulated rig^d body can be used 
to track the orientation of each limb. In such systems, body posture can be tracked 
and introduced into a synthetic enviromnent, thereby allowing a user to interfece with 
the synthetic environment. 

The Nature of the Mathematical Problem 

20 A barrier to an effective implementation of effective body tracking systems 

stems fi:om certain mathematical difficulties inherent in mapping body orientation 
firom one coordinate system to another. These difficulties have presented problems 
which convOTtional approaches have not been able to resolve for over 100 years. 

The nature of the problem is briefly outlined in the foUowmg paragrj?>hs. A 
25 conventional way of describing the orientation of a rigid body uses '*Euler angles" to ^ 
describe the orientation of a rigid body in three dimensions. Euler angjes describe the 
orientation of a rigid body using three rotations about specified axes. One commonly 
used reference coordinate system is the local **flat Earth" system. Such reference 
systems use an arbitrarily selected origin on the surface of the earth with respect to 
30 coordinate axes X, Y, and Z. By convention, the X-direction corresponds to local 
north, the Y-direction corresponds to local east, and the Z-direction corresponds to 
down as depicted in Fig. 2(a). Additionally, it is important to specify a body 
coordinate system which is attached to the body being tracked. Fig. 2(b) depicts such 

7 
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10 



15 



a system. This is also an X-Y-Z system with X pointing "out of the nose" in a 
positive direction, Y out the right side, and Z down. The subscript **E" designates 
earth reference coordinates (Xe, Ye, Ze) and the subscript *TB" designates body 
reference coordinates (Xb, Yb, Zb). Euler angles represent the orientation of a rigid 
body using three rotations about specified axes in a specified order of rotation. For 
example, referring to Fig. 2(a), first rotation about the north axis, second rotation 
about the east axis, and third rotation about the down axis. These would be analogous 
to 'Voir*, "elevation", and "azimuth". By convention, roll angle is designated by 
elevation by "6", and azimuth It is to be noted that if the temporal order of 
rotations is reversed, body axis rotations yield exactly the same orientation as 
reference axis rotations. 

The position of a point in space can be described using a three-dimensional 
point vector. A rigid body can be described in terms of a plurality of point vectors. 
An example vector can be represented by a vector V = [x y z]. In order to describe 
rotational behavior, matrix transforms are used. For example, if a vector is rotated 
about an angle <p (e.g., about the XE-axis described with respect to Fig. 2(a)), the 
following rotation transform can be used. 



IK OKKKO 
OK cos^K -sin^ 
OK sin^K cos^ 



= [rot{x,<p)] 



Thus, the original x, y, z coordinates can be translated into the rotated 
20 coordinates x', y\ z' through the ^plication of a 3x3 rotation matrix. In the depicted 
example, the 3x3 matrbc is designed to accomplish a rotation about the X-axis 
(represented by [rot(x, (p)\ Similar 3x3 rotation matrices exist for 0 and ^ rotations 
about the Y- and Z-axis, respectively. Such rotation transforms are known to those 
having ordinary sldll in the art and will not be described here in great detail. 
25 Additionally, a single 3x3 rotation matrix can be xised to describe rotation about all 
three axes. One example of such a rotation matrix is shown below. 



-V = [ro^(z,v^)][roKy,^)][roK^,^)r v= 
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"(cosy^cos^)K (cosyrsintf sin^-siin^cosfl7)K (cosij/^sindcos^+sini^sin^) 
(sin;^cos0)K (cosj^cos^ + siii5^sin5sm$?)K (cos 5^ sin 9? + sin sin ^ cos 9?) 
(-sin)K K K K (cos5 sin^)K K K K K K K K (cos ^ cos^) 

As can be seen above, one of the difficulties inherent in using Euler angles and 
the aforementioned transform matrices is the sheer number of computations required 
5 to calculate each motion of a body. The problem becomes magnified as more and 
more bodies are simultaneously tracked. In the end, the complex calculations 
required using these existing algorithms require so much time that they can not 
effectively be used to track body position and orientation in real-time. This is a 
significant drawback. 

0 In addition, Euler angle systems present certain computational difficulties. In 

particular is the problem of singularities. Singularities result when a sensor (or the 
rigid body to which it is attached) is tilted to a 90° (degree) angle. For example, if a 
rigid body is tilted such that it points straight up. At such a point, the roll and azimuth 
axes are co-linear. This results in a situation where neither the roll nor azunuth angles 

5 are uniquely defined, only their difference or sum can be specified uniquely. This 
problem becomes magnified when the time rate of change of Euler angles are used to 
quantify rates of angular rotation. This singularity problem is known to those having 
ordinary skill in the art and presents significant difficulty to conventional body 
tracking systems. 

0 The principles of the present invention use magnetometer and accelerometer 

input subject to filtering to track body posture. In one implementation, Euler angles 
and their related coordinate transform matrices are used to calculate body orientation. 
Although computationally intensive, embodiments of the present invention can use 
Euler angle calculations to track body orientation. Additionally, angular velocity 

15 information can be used to correct for time lag errors. However, other embodiments 
of the present invention present a particularly advantageous approach for achieving 
body tracking using quaternion mathematics. Such embodiments eliminate the Euler 
angle singularity problem, and reduce the computational complexity of the invention. 

In accordance with the principles of the present invention, sensor signals are 
;0 input into an attitude estimation filter. Such a sensor can function using ordinary 
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Euler angle mathematics. But, in preferred embodiment, the filter is a quaternion 
based complementary attitude estimation filter. Using quaternion mafliematics results 
in an approximate 100- fold increase in processing efficiency. Although the present 
implementation is novel, the field of quaternion mathematics is known to those 

5 having ordinary skill in the art and is explained in detail in numerous mathematical 
texts. One example, is Kuipers, J, "Quaternions and Rotation Sequences", Princeton 
University Press, Inc., Princeton, NJ, 1998 (hereinafter "Kuipers"), which is hereby 
incorporated by reference. 

Such a filter is used in conjunction with data suppUed by sensors to produce a 

10 sensor orientation estimate repressed in quaternion form. In one embodiment, the 
sensors include a three-axis magnetometer and a three-axis accelerometer. In another 
sensor embodiment, the magnetometers and accelerometers are supplemented with 
angular rate detectors configured to detect the angular velocity of the sensor 
(comprising so-called Magnetic, Angular Rate, Gravity (MARG) sensors). Each 

15 MARG sensor contains angular rate detectors, accelerometers, and magnetometers. 
Estimation error is minimized using Gauss-Newton iteration. Unlike, other smsors 
known in the art, sensor embodiments of the invention can correct for drift 
continuously without any requirement for still periods. 

In an articulated rigid body, posture is determined by estimating individual 

20 limb segment orientations through the attachment of sensors. The orimtation 
estimates are used to animate a simple human model or avatar in real-time. The 
model directly accepts limb segment orientation information in quaternion form 
relative to a fixed referrace fi:ame (for example, an earth fixed reference firame). 
Simple calibration procedures are used to adjust sensor scale factors and null points, 

25 as well as account for offeets between the sensor coordinate fi-ames and the firames 
associated with the limb segments to which they are attached. 

Bodv Tracking Using Sensors with Magnetometers and Accelerometers 
Referring to Fig. 3, q defines the variable for the orientation quaternion and q 
30 defines values for the orientation quaternion. Accelerometers measure and return an 
approximation to the local gravity vector (the local vertical), the unit vector A 31 . The 
magnetometers measure and return the direction of the local magnetic field vector (the 
unit vector b) 32. 



10 
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Jn short, magnetometers can be used to measure the direction of the local 
magnetic field vector and accelerometers can be used to measure the direction of the 
local gravity vector. In one example, accelerometers 31 can be used to detemiine the 

local gravity vector by measuring the combination of forced linear acceleration and 
the reaction force due to gravity. As such accelerometer data can be used to 
detemiine a local gravity vector. That is because the accelerometer measures a total 
acceleration vector A'measured defmed by 

In one embodiment, a three-axis accelerometer can be used to measure total 
acceleration (forced linear acceleration and gravitational reaction force) ^'measured over 
a fixed time period. By conducting a time weighted summing (or integrating) of 
acceleration values over some relatively short time period the accelerations and 
decelerations exerted upon the body should average to zero. Time weighted summing 
methods emphasize the most rec^ measurements with respect to measurements 
taken in the past. Such methods are well known to those having ordinary skill m the 
art However, the effects of gravitational acceleration exerted on the body do not 
average to zero. Thus, components of Ihe gravity vector can be determined. This 
works particularly well for objects (bodies) that undergo accel^ation and deceleration 
on a relatively short time firame. For example, where a sensor is mounted on a 
forearm and is moved in normal course of motioiL Determination of this local gravity 
vector allows the local vertical to be determined allowing correction of orientation 
relative to a vertical axis. Similarly, magnetometers 32 measure the local magnetic 
field in body coordinates. This information can be used to correct rate sensor drift 
errors in the horizontal plane. Thus, the vectors derived firom accelerometer and 
magnetometer data comprise a method of determining orientation. 

The magnetometer returns a local magnetic field vector (the unit vector b) in 
sensor coordinates. The accelerometer returns a local gravity vector (the unit vector 
h) in sensor coordinates. These two vector quantities b and h, expressed in sensor 
coordinates as piire vector quaternions, are imit vectors 

A = [0 h, Ji, h,] (2) 

6 = [0 b, b, 63] (3) 
The vector parts firom Eqns. (2) and (3) can be combined to produce a 6 x 1 
measurement vector yo 34 in sensor coordinates: 

11 
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y,=[h, h, h, b, b, bJ (4) 
In addition, it is known that gravity in earth coordinates is always down and 
can be expressed as the down unit vector in quaternion form as 

iw = [0 0 0 1] (5) 
5 Also, the local magnetic field in earth coordinates can be determined and 

normalized and can be expressed in quaternion form as 

« = [0 n^] (6) 

As is known to those having ordmary skill in the art (e.g., See, Kuipers), Eqns. 
(5) and (6) can be mapped from the earth fixed firame to the sensor fi^ae througji 
10 quaternion multiplication (See, Fig. 3, Block 35) by 

h = g''^niq b = q'^nq (7) 
Combinmg the vector parts of Eq. (7) yields a single 6x1 computed 
measurement vector ^{q) 35a,wherem: 

y{q)AK h h bx h bJ (8) 
1 5 and wherein the values for h^.h^yh^.b^.b^, 63 are generated by mapping m and n 
through as an estimated orientation quaternion. 

Then the difference between the actual measurements yo and the computed 
measurement vector is defined as the error vector 

^(<7) = i^o-i^(«) (9) 
20 In viewuig Eqa 9, it is noted that if in EqrL 8 there is no measurement noise, 

the mininniTp difference between the measured and computed values will equal the 
zero vector. 

The square of the error vector (Eq. 9) is termed the criterion Junction 

<p{q)^l\q)l{q) (10) 

25 The criterion function is a scalar which can be minimized 38 (also referred to 

herein as criterion Amotion minimization by filtering). In one filter embodiment, the 
error vector is minimized by minimized the criterion fimction using Gauss-Newton 
iteratioxL The details of a Gauss-Newton iteration are known to those having ordinary 
skill in the art. One example of such an implementation is described in McGhee, R., 

30 "Some Parameter-Optimization Techniques," Digital Computer User's Handbook, 
McGraw-Hill, pp. 234-253, 1967, (hereinafter "Handbook**) hereby incorporated by 
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reference. This method is based on linearized least squares regression analysis where 
is considered a vector of data points and y{q) is a vector to be fitted to those points. 
The forgoing filter can be implemented using sensors having magnetometers 31 and 
accelerometers 32. Alternatively, other filtering embodiments can be employed 

5 including, but not hmited to least squares filtering, Wiener filters, Kahnan filters can 
be used. Such sensor and filter systems provide suitable method and apparatus for 
determining orientation (posture) of rigid bodies and articulated rigid bodies- The 
output of the filters can be integrated 42 and normalized 43 to provide an estimated 
orientation quaternion 39. However, due to magnetometer 31 and accelerometer 32 

10 measurement inaccuracies, sensor drift error, and time lag present in such filtering 
systems improvements can be in the accuracy of such a system. 

Body Tracking Using MARG Sensors 

The accuracy of such method and system embodiments can be enhanced by 
using sensor angular velocity data supplied by an angular rate detector. Fig. 3 depicts 

15 the inputs &om sensor embodiments that include angular velocity (rate) detectors 33. 
Interpreted in this way, such a filtering embodiment measures angular rate 
information 33, and uses measurements of local magnetic field 32 and local gravity 31 
to correct the angular rate information or mtegrated angular rate information. 
The angular rate detectors 33 provide angular rate information 37 to the 

20 filtering system. Thus, as with the previously discussed embodiment, accelerometers 
return an approximation to the local gravity vector A 3 1 and the magnetometers return 
the direction of the local magnetic field vector b 32. Again, a 6 x 1 measurement 
vector yo 34 in sensor coordinates is produced (Eqn. (4)). Again, in accordance with 
Eq. (7), Eq. (5), and Eq. (6) are approximations mapped fi:om the earth fixed firame to 

25 the body firame through quaternion multiphcation 35. And a 6 x 1 computed 

measurement vector p(q) 35a is generated. As previously described, the difference 
between the measurement vector yo and the computed measurement vector }f(q) is the 
error vector ^{q)36 and the square of flie filter modeling error is termed the criterion 
Junction, The error vector is then minimized. For example, using Gauss-Newton 

30 iteration. 

In such an embodiment the filter inputs are firom a three-axis accelerometer 
(hu hi, hi) 31, a three-axis magnetometer (fei, 62, b^ ) 32, and a three-axis angular rate 
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sensor (p, q, r) 33. Its output is a quaternion representation of the orientation of the 
tracked object q 39. 

In this embodiment, such magnetometer and accelerometer data (32, 31, 
respectively) can be considered complementary to sensor angular rate 33 data. Such 
5 angular rate data 33 can be used to describe sensor orientation. If the mput from the 
angular rate sensor 33 were perfect (i.e., accurate, noiseless, and imbiased) it could be 
directly processed to obtain a rate quaternion 

#=-9®(0,A?,r)=:Vfl> (11) 
2 2 

where ^ is a quaternion representing the current orientation, the indicated product is a 
10 quaternion product and the superscript S means measured in the sensor reference 

frame. Single integration 42 of ^is normalized 43 to produce a quaternion q , which 
describes new value for estimated orientation of the sensor prior to normalization 43. 
However, in normal operating environment, the output 33 of angular rate detectors 
tends to drift over time. Thus, rate detector data 33 can be used to determine 

1 5 orientation only for relatively short periods of time unless this orientation is 

continuously corrected using "complementary" data from additional sensors (here, 
accelerometer 3 1 and magnetometer 32). Thus, as previously ^plamed with respect 
to Eqns (9) and (10) a Gauss-Newton iteration 38 is performed to correct a measured 
rate quaternion (See, Fig 3, 36). 

20 A full correction A q&u 40, if applied to tiie measured rate quaternion, can be 

defined by 

ISq^ ^[X'xYx's{q) = S-'X's{q) (12) 

where q is the previous estimate for q and the ^matrix is defined as 

25 Such an X matrix is described in great detail in McCSiee, R., Bachmaim, E., 

Yun X. & Zyda, M. "Real-Time Tracking and Display of Human Limb Segment 
Motions Using Sourceless Sensors and a Quaternion-Based Filtering Algorithm - Part 
I: Theory," MOVES Academic Group Technical Report NPS-MV-01-001, Naval 
Postgraduate School, Monterey, CA. 2000 (Hereinafter, the Theory Paper) which is 

30 hereby incorporated by reference in its entirety. 
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Eq. (12) treats m and /i as if they are perfect measurements of gravity and the 
local magnetic field. Inreality, such data is firequendy corrupted by noise. This can 
be corrected (41 Fig. 3) by using a scalar multiplier as defined by 

Aq^^alX^XyX^l{q) (14) 

5 where 

a-^kAt (15) 
and k represents the filter gam value 41. Thus, for discrete time step integration, the 
next estimate of sensor orientation would be 

§.,.=^.+i§ 'a>t^^a[x'xXx's{g„) ^^^^ 

Thus, such filtering systems can be thought of as time weighted filtering 
systems, because newer measurements are weighted more heavily than measurements 
taken more distant in the past 

Reduced Order Filtering 

If q is not constrained to unit length as depicted in Figure 3, a unique solution 
to the optimization problem no longer exists and the A'matrix will not be of fiill rank. 

In this case the regression matrix 

S = X^X (17) 

will be singular and can not be inverted. 

A more efficient alternative to the computation of Aq results firom noting that 

if 

^««r=$«w+A^>w; (18) 
and if both and q^ are unit quaternions, then any small must be orthogonal 
to q. That is, the oiily way to alter a unit vector while maintaining umt length is to 
rotate.it, and for small rotations Aq must be tangent to the unit four-dim^isional 
sphere defined by 

q^q-\gt-l (19) 
where q* is the conjugate of q (See, previously incorporated reference by Kuipers). 
From the Ortho^nal Quaternion Theorem (See, the previously referenced Theory 
Paper), ifp and q are any two quaternions, then p is orthogonal to q if, and only if,p is 
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15 



20 



25 



the quaternion product of q and a unique vector v (real part equal to zero) where v is 
given by 

v = q''(S>p (20) 
Accordingly Aq can be written in the form 

A4r = g®v = ^®(0 Vi Vj Vj) (21) 
With this constraint, linearization of the computed measurement vector, y (9) , in 
Figure 3, yields 

(22) 



10 and consequently: 



|. = ^(g®(0 1 0 0))=^(^®iy (23) 
|L = jr(g0(O 0 1 0))^X{q^jf (24) 

^2 



and 



^ = X{g^{0 0 0 l))^X{q^kY (25) 

Thus, when Gauss-Newton iteration is applied to unit quatemions, it is sufiSdent to 
solve for only three unknowns rather than four as in the methods for estimation of ' 
Aqji^ in Eq. (12). That is, if X is the 6 x 3 matiix 



1 dy 


dy' 







then. 



and 



(26) 

(27) 
(28) 



Incorporation of the above into the Gauss-Newton algorithm, notably 
simplifies the computation of the ^ q quaternion since it requires only a 3 x 3 matrix 
mversion rather than the 4 x 4 matrix inversion of the basic algorithm. As is known to 
those having ordinary skill in the art, this is relevant since best algorithms for matrix 
inversion are of 0(n^) complexity. 



16 



wo 02/37827 



PCT/USOl/46226 



Jn an alternative approach, a reduced order X matrix (See, e.g., 38 Fig. 3 or 
Eq. 12) can be implemented Such a matrix can be described as: 



The derivation of this matrix is explained in R3. McGhee, Bachmami, E.R. 

5 X.P, Yun, and M. J. Zyda, "An Investigation of Alternative Algorithms for Singularity- 
Free Estimation of Rigid Body Orientation from Earth Gravity and Magnetic Field 
Measurements", Ph.D. dissertation, Naval Postgraduate School, Monterey, CA., 2001, 
which is hereby incorporated by reference. The yx values are the y values computed 
for the computed measurement vector ^(g) . In addition, the yx values can be 

10 calculated from any set of measured magnetic field vector values and gravity vector 
values in sensor coordinates. Such sets can be randomly determined or in accordance 
witili some predetermined value. Also, the measured values of the magnetic field 
vector and gravity vector can be used to generate computed measurement vector 
^{g) and those y values can be input into the X matrix of Eqn, 29. 



Weighted Least Squares Regression 

With reference to Fig 3 (at 36), detector data can be weighted puttmg greater 
or less reliance data received from the detectors 31, 32, 33. For example, greater or 
less reliance may be placed on magnetometer data 32 in comparison to accelerometer 
20 data 3 1 . This could come about because one or the other of these signals could prove 
to be less accurate (or noisier) than the other. This can be achieved by merely 
redefining the error vector, 5 , as: 



X^2 



0 -y, y2 

-yi yx 0 

0 -3^6 y$ 

y^ 0 -y. 



(29) 



15 



p{yor yiA) 
p{yo, -yM 
p{yfk^ y(^)s\ 



(30) 
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In such an embodiment, setting p > i emphasizes magnetometer data, while 0 < p < i 
puts greater weight on accelerometer data. This change alters the X matrix by 

multiplying the last three elements of each column by . If a detailed statistical 
model is available for magnetometer and accelerometer errors for a particular 
5 experimental setting then, at least conceptually, the best value for p could be 
obtained from linear estimation theory. In the absence of such statistical data, it is 
probably more productive to think of p as a 'tunable" parameter adjusted by "a best 
guess approximation optimization" in a given situatiorL 

Fig. 4 shows one embodiment of an overall system implementation in 

10 accordance with the principles of the present invention. In the depicted embodiment 
three sensors are used to track the posture of a human body. Embodiments using 
fewer or greater numbers of sensors can be used. Typically, one sensor is attached to 
each limb segment to be tracked. The exact number of sensors used depends upon the 
degree of detail (resolution) desired by the user of such motion tracking devices. 

15 By mounting a pluraMty of sensors on a body, the posture of the body can be 

determined and tracked. Sensors constructed in accordance with the principles of the 
present invention can be used to track motion and orientation of simple rigid bodies as 
long as they are made of non-magnetic materials. Examples include, but are not 
limited to hand-held devices, swords, pistols, or simulated weapons. However, the 

20 inventors contemplate using the principles of the present invention to track the posture 
of articulated rigid objects, in one example, human bodies. Such articulated rigid 
bodies feature a plurality of segments interconnected by a plurahty of joints. Each of 
the segments can correspond to, for example, limbs and extremities such as head, 
hands, forearms, legs, feet, portions of the torso, and so on. The joints corresponding 

25 to wrist, elbow, shoulder, neck, backbone, pelvis, knees, ankles, and so on. The 
inventors contemplate the application of these principles to other articulated rigid 
body embodiments. For exanq)le, non-magnetic prosthetic devices, robot arms, or 
. other machinery can by tracked in accordance with the principles of the present 
invention. Additionally, animal body motion can be tracked using such devices. 

30 Fig. 4 is a figurative diagram of an overall system embodiment capable of 

detecting and tracking and orientation and posture of a rigid articulated body 402. 
One or more sensors 401 are positioned on a rigid articulated body 402. Such sensors 
401 detect the posture of the articulated rigid body 402, This sensor information is 
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output to a processing unit 403 (for example, a microprocessor configured as a central 
processing unit (CPU) of a computer) that calculates the posture of the articulated 
rigid body 402. This information can be transmitted from the sensors 401 to the CPU 
403 using wires connected to the CPU 403 through appropriate interface circuitry. 
5 Such circuitry can include interface card, and if necessary I/O comiection boards, and 
A/D cards. Alternatively, the sensor information can be transmitted to the CPU using 
wireless communication devices. In some embodiments, the sensors can include 
microprocessors that can accomphsh much of the function of the interface circuitry or 
the posture calculation. Moreover, the CPU 403 can be incorporated into a small unit 

10 which can be carried on the body 402. Also, alternatively sensor information can be 
transmitted to a body mounted unit that then transmits the sensor information to the 
CPU 403 using, for example, wireless communication. 

The CPU 403 then calculates the body posture and outputs a display signal to 
a display 404 (for example virtual reality display goggles or more conventional 

IS display devices such as monitors), thereby enabling the movement of the articulated 
rigid body 402 to be incorporated into a synthetic or virtual environment and then 
displayed. Where the movement being tracked is that of a non-magnetic simple rigid 
body (e.g., a simulated rifle or some like implement) the system is simplified, peihaps 
requiring only a single sensor 401 to track the motion of the rifle. 

20 Sensor embodiments capable of incorporating the above-described principles 

can include a magnetic field detector and a gravitational field detector. In some 
embodiments micro-electrb-mechanical system (MEMS) technology can be used to 
construct suitable devices. Other embodimrats further include angular rate sensors. 
One example of a suitable sensor device is an analog MARG sensor. Ih one 

25 embodiment such a sensor measures 10.1 x 5.5 x 2.5 cm. The analog output of the 
sensor is connected to a breakout header via a thin VGA monitor cable. Output range 
is 0-5 vdc. The power requirement of the sensors is 12 vdc at approximately 50 
milliamperes. The primary sensing components are a triaxial accelerometer (e.g. a 
Model No. CXL04M3 manufactured by Crossbow, Inc.), a 3-axis magnetometer (e.g., 

30 Model No. HMC2003 from Honeywell), and three miniature angular rate detectors 

mounted in an orthogonal configuration (e.g., Tokin CG-16D series sensors available 

from Tokin American, Inc.). The individual components can be integrated using a 

single integrated circuit board with the accelerometers mounted separately. Rate 

sensor output voltage is amplified by a factor of five and filtered to attenuate rate 
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sensor oscillator noise. Such MARG sensors can be obtained from McKinney 
Technology of Pnmedale, Cahfomia. Software or hardware biasing canbe used to 
successfully integrate the signal from the angular rate detectors with the rest of the 
system. In one embodiment flie angular rate signal can be passed through 
5 conditioning circuitry using capadtive coupling. 

m order for the system to operate properly, the MARG sensors must be 
caUbrated prior to posture tracking. Advantageously, unless the characteristics of the 
sensors themselves change, calibration need only be accomplished once. However, 
due to variations in the directionofthelocalmagnetic field vector, betterperfonnance 

10 is to be expected if the direction of this reference is determined before each tracking 



20 



25 



session. 



30 



Accelerometeis can be calibrated by placing them in a vertical position to 
sense gravity in one direction and then turning it over to sense gravity in the other. 
Halfway between the readings taken is the null point. 

„ accd max + accelmm 
accelnull =— :j 



j5 acc&inuu^^ ^ 



MultipUcation of a correct scale factor times the accelerometer output values 
willresultinaproductofl gin one direction and -1 ginihe other. This scale fector 

can be found using 

{accelunits)-Kl /22") 

A method of magnetometer calibration is very similar to that used for 

accelerometers. Instead of orienting each sensor relative to the gravity vector, each 
magnetometer is positioned to sense the maximum strength oftiielocalmagnetic field 

along both its negative and positive axes. 

Determination of the null point of an angular rate detector is achieved by 
measuring the output of a static angular rate detector, tiien averaging the readings. 
Scale factors are tiien esthnated by integrating the output of angular rate detector as it 
is subjected to a known angle of rotation. The scale factor for a rate detector can tiien 
be determined following a known rotation using 

known rota tion 
''^''^'^'"'esHmatedrotation ^ 

where the estimated rotation term is tiie result of integrating tiie output of tiie 
detector with a scale factor of unity. 
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The forgoing method can calibrate a MARG sensor using a level platform and 
a simple compass to indicate the direction of the local magnetic field. Each sensor 
can be calibrated by placing it in six positions allowing each accelerometer to sense 
gravitation acceleration in both the positive and negative directions, subjecting each 
5 rate detector to one or more known rotations and rotating the MARG sensor in a 
manner such that maximum and minimum local magnetic field readings could be 
obtained for each magnetometer. 

The vertices of an individual segment of a human model can be described 
relative to a coordinate system having a z-axis in a down direction (for example, as 

10 shown in Fig. 2(a)) that is attached to the inboard (proximal) end of the segment. 
Such systems are known to those having ordinary skill in the art. See, for example, 
Bachmann, E., "Ihertial and Magnetic Angle Tracking of Limb Segments for Inserting 
Humans into Synthetic Environments", Ph.D. dissertation. Naval Postgraduate 
School, Monterey, CA., 2000, which is h^eby incorporated by reference. 

15 By aligning the coordinate axes of a sensor and a limb segment, the orientation 

of an individual limb segment can be £;et by applying to each vertex, v , the quaternion 
rotation 

q^^^y^q^ (34) 

where the unit quaternion qsensor is the estimated orientation produced by the filter 

20 processing the sensor output. 

Misahgmnent between the sensor and limb segment axes can be taken into 
account by performing an additional fixed rotation using an offset quaternion 
^s««r<^(^.i^®v®^;^)®^l^ (35) 

to each vertex, where qcffset is the ofifeet quaternion for the limb of the vertex. 

25 When the human model is in the reference position, the limb segment 

coordinate axes are aligned with the corresponding Earth-fixed axes. That is the x- 
axis for each limb segment pomts toward the local north, the y-axis points east and the 
z-axis points down. The offeet quaternion for each limb segment can be derived by 
noting that while the user is in the reference position the equation 

30 v=9^®9^®v®^;^0^:^ (36) 

holds true. 

Compensation for the way in which all sensors are attached to the limbs of a 
tracked subject can be accomplished by simply setting qoffset for each limb segment to 

21 
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the inverse of the associated qsensor'^i^Q the subject to be tracked is standing in a 
predetermined reference position. 

To set the position of an individual limb segment, it is necessary to find a 
vector that describes the location of the inboard end of the limb segment. Once this 
5 vector is found, the final position of each vertex can be calcxilated through addition of 
this vector to the rotated coordinates of each vertex. Thus, the final position of a limb 
segmoit vertex is given by 

Ptra» -^q^ ®(^^er ®V<^q^)^q^ (37) 

where p/ronf is the vector sum of rotated translation vectors associated with limb 

10 segmaits that are between the body origin and the lunb segment being positioned. 
Figs. 5 and 6 are block diagrams illustrating certain aspects of method 
embodiments of the present invention. Fig. 5 describes a method embodiment for 
motion tracking using angular rate detector information complemented with magnetic 
vector and gravity vector information. The angular rate detector measures an angular 

15 velocity of the sensor to generate angular rate values (501). These angular rate values 
are integrated (503) and normalized (505) to produce an estimate of sensor 
orientation. A magnetic field vector is measured to generate local magnetic field 
vector values (507). An acceleration vector is measured to generate local gravity 
vector values (509). A measurement vector is determined fi:om the local magnetic 

20 field vector values and the local gravity vector values (511). A computed 

measurement vector is calculated firom the estimate of sensor orientation (513). The 
measurement vector is compared with the computed measurement vector to generate 
an enor vector that defines a criterion function (515), A mathematical operation is 
then performed resulting in the minimization of the criterion function and the 

25 gmeration of an error estimate (517). The error estimate is mtegrated (5 1 9) and 
normalized (521) to produce a new estfanate of sensor orientation (521). In general, 
these new estimates of smsor orientation are output as sensor orientation signals that 
can be used to track the orientation of the sensor (525). The operations of (501-525) 
are rq)eated using the new estimate of sensor orientation for (513) calculating a 

30 coii5>uted measuronent vector (523). Such process continually outputting sensor 
ori^tation information and receiving new detector input to adjust sensor orientation 
information until tracking is no longer desired. The details of executing each of these 
operations has been explained hereinabove. 
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Fig. 6 describes another method embodiment for motion tracking using 
quaternion mattiematics. The method begins by providing a starting estimate of 
sensor orientation quaternion (601). As previously explained, the starting estimate 
can be any estimate of sensor orientation including a randomly chosen estimate of 
sensor orientation. The magnetic field vector and an acceleration vector are measured 
to generate local magnetic field vector values (603) and local gravity vector values 
(605). These values are used to determine a measurement vector (607), A computed 
measurement vector is then calculated from the estimate of sensor orientation using 
quaternion mathematics (609). The measurement vector is compared with the 
computed measurement vector to generate a 6x1 error vector that defines a criterion 
function (61 1). A mathematical operation is performed that results in the 
minimization of the criterion fimction and outputs a 4x1 quaternion error estimate 
(613). As previously explained, this mathematical operation can using the X matrix, 
for example, by multiplying [X^ X]"* X^ with the error vector. As is known to 
persons having ordinary skill in the art other operations can be used. The 4x1 
quaternion error estimate is integrated (615) and normalized (617) to produce a new 
estimated sensor orientation quatemioa This new estimated sensor orientation 
quaternion can be ou^ut as a sensor orientation signal and used for tracking (621). 
Also, the operations of 601-621 are repeated, wherein the new estimated sensor 
orientation quaternion is used for calculating (609) a computed measurement vector 
(619). Such process continually outputting sensor orientation quaternions and 
receiving new detector input to adjust sensor ori^tation quaternions until tracking is 
no longer desired. The details of executing each of these operations has been 
explained hereinabove. 

The present invention has been particularly shown and described with respect 
to certain preferred embodiments and specific features thereof However, it should be 
noted that the above described embodiments are intended to describe the principles of 
the invention, not limit its scope. Therefore, as is readily apparent to those of 
ordinary skill in the art, various changes and modifications in form and detail may be 
made without departing firom the spirit and scope of the invention as set forth in the 
appended claims. Other anbodiments and variations to the depicted embodiments 
will be 25)parent to those skilled in the art and may be made without departing fix)m 
the spirit and scope of the invration as defined in the following claims. In particular, 
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it is contemplated by the inventors that the principles of the present invention can be 
practiced to track the orientation of simple rigid bodies such as weapons or simulated 
weapons. Additionally, the inventors contemplate tracking the posture of many types 
of articulated rigid bodies including, but not limited to prosthetic devices, robot arms, 

5 moving automated systems, and living bodies. Also, the inventors contemplate other 
filtering embodiments including, but not limited to Weiner and Kalman filters. 
Further, reference in the claims to an element in the singular is not intended to mean 
"one and only one" unless expUcitly stated, but rather, "one or more". Furthermore, 
the embodiments illustratively disclosed herein can be practiced without any element 

10 which is not specifically disclosed herein. 
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CLAIMS 

We claim: 

5 1 . A method of tracking the orientation of a sensor, the method comprising: 

a) measuring an angular velocity of the sensor to generate angular rate 

values; 

b) integrating the angular rate values; 

c) normalizing the integrated angular rate values to produce an estimate 
10 of sensor orientation; 

d) measuring a magnetic field vector to generate local magnetic fidd 
vector values; 

e) measuring an acceleration vector to g^erate local gravity vector 
values; and 

15 £) correcting the estimate of sensor orientation using the local magnetic 

field vector and local gravity vector. 

2. A method of tracking as in Claim 1 wherein correcting the estimate of sensor 
orientation using the local magnetic field vector and local gravity vector comprises: 
20 g) deteraiining a measurement vector &om the local magnetic field vector 

values and the local gravity vector values; 

h) calculating a computed measurement vector &om the estimate of 
sensor orientation; 

i) comparing the measurement vector with the computed measurement 
25 vector to generate an error vector that defines a criterion fimction; 

j) performing a mathematical operation that results in the minimization 
of the criterion function and outputs an error estimate; 
k) integrating the error estimate; 

1) normalizing the integrated error estimate to produce a new estimate of 
30 sensor orientation; and 

m) repeating steps a)-m), wherein the new estimate of sensor orientation is 
used for h), calculating a computed measurement vector until tracking is no longer 
desired. 
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3. The method of Claim 2 wherein the operation of j), perfonning a mathematical 
operation that results in the minimization of the criterion function comprises 
minimizing the criterion function without calculating the criterion function. 

5 4. The method of Claim 2 wherein the operation of j), perfonning a mathematical 
operation that results in the minimization of the criterion function includes 
implementing a partial correction step to compensate for measurement error. 

5. The method of Claim 4 wherein implementing the partial correction step to 

10 compensate for measurement error is supplemented by using a weighted least squares 
regression to emphasize more reliable measurements with respect to less reliable 
measurements. 

6. The method of Claim 2 wherein the operation of j), perfonning a mathematical 
15 operation that residts in the minunization of the criterion function comprises using 

time weighted Sitting. 

7. The method of Claim 2 wherein the operation of g), perfonning a 
mathematical operation that results in the minimization of the criterion function 

20 comprises using a Gauss-Newton iteration. 

8. A method of tracking the orientation of a sensor, the method comprising: 

a) measuring an angular velocity of the sensor to generate an angular rate 
quaternion; 

25 b) integrating the angular rate quaternion; 

c) normalizing the integrated angular rate quaternion to produce an 
estimated sensor orientation quaternion; and 

d) measuring a magnetic field vector to generate local magnetic field 
vector values; 

30 e) measuring an acceleration vector to generate local gravity vector 

values; 

f) correcting the estimated sensor orientation quaternion using the local 
magnetic field vector and local gravity vector. 
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9. A method of tracking as in Claim 8 wherein correcting the estimated sensor 
orientation quaternion using the local magnetic field vector and local gravity vector 
comprises: 

g) determining a measurement vector fix)m the local magnetic field vector 
5 values and the local gravity vector values; 

h) calculating a computed measurement vector firom the estimated sensor 
orientation quaternion; 

i) comparing the measurement vector with the computed measurement 
vector to generate an error vector that defines a criterion function; 

10 j) performing a mathematical operation that results in the rniriimization 

of the criterion function and outputs an error estimate quaternion; 
k) integrating the error estimate quateroion; 

1) normalizing the integrated error estimate quaternion to produce a new 
estimated sensor orientation quaternion; and 
15 m) repeating steps a)-mX wherein the new estimated sensor orientation 

quaternion is used for h), calculating a computed measurement vector. 

10. The method of Claim 9 wherein the operation of j), performing a mathematical 
operation fliat results in ttie minimization of the crit^on function comprises 

20 TTiiniTnizing the criterion function without calculatmg the criterion function. 

1 1 . The method of Claim 9 wherein the operation of j), performing a mathematical 
operation that results in the minimization of the criterion function includes 
implemmting a partial correction step to compensate for measurement error. 

25 

12. The method of Claim 10 wherein implementing the partial correction step to 
compensate for measurement error is supplemented by using a weighted least squares 
regression to emphasize more reliable measurements with respect to less reliable 
measurements. 

30 

13. The method of Claim 9 wherein the operation of j), performing a mathematical 
operation that results in the minimization of the criterion function comprises using 
time weighted filtering. 
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14. The metiiod of Claim 9 wherein the operation of g), performing a 
mathematical operation that results in the minimization of the criterion function 
comprises using a Gauss-Newton iteration. 

5 15. A method of tracking the orientation of a sensor, the method comprising: 

a) providing a starting estimate of sensor orientation; 

b) measuring a magnetic field vector to generate local magnetic field 
vector values; 

c) measuring an acceleration vector to generate local gravity vector 

10 values; 

d) determining a measurement vector from the local magnetic field vector 
values and the local gravity vector values; 

e) calculating a computed measurement vector firom the estimate of 
sensor orientation; 

15 f) comparing the measurement vector with the computed measurement 

vector to generate an error vector that defines a criterion function; 

g) performing a mathematical operation that results in the minunization 
of the criterion function and outputs an error estimate; 

h) integrating the error estimate; 

20 i) normalizing the integrated error estimate to produce a new estimate of 

sensor orientation; and 

j) repeating steps a)"j), whCTein the new estimate of sensor orientation is 
used for e), calculating a computed measurement vector. 

25 16. The method of Claim 1 5 wherein each new estimate of sensor orientation is 
output as a sensor orientation signal. 

17, The method of Claim 15 wherein the operation of g), performing a 
mathematical operation that results in the minimization of the criterion function 

30 comprises minimizing the criterion function without calculating the criterion functiorL 

18. The method of Claim 15 wherein the operation of g), performing a 

mathematical operation that results in the minimization of the criterion function 

includes implementing a partial correction step to compensate for measurement error. 
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19. The method of Claim 1 8 wherein implementing the partial correction step to 
compensate for measurement error is supplemented by using a weighted least squares 
regression to emphasize more reliable measurements with respect to less reliable 

5 measurements. 

20. The method of Claim 1 5 wherein the operation of g), performing a 
mathematical operation that results in the minimization of the criterion function 
comprises using time weighted filtering. 

21 . The method of Claim 15 wherein the operation of g), perfonning a 
mathematical operation that results in the minimization of the criterion function 
comprises using a Gauss-Newton iteration. 

22. The method of Claim IS wherein the operation of g), performing a 
mathematical operation that results in the minimization of the criterion function and 
outputs an error estimate includes: 

measuring an angular velocity of the sensor to generate angular rate values; 
integrating the angular rate values; 

normalizing the integrated angular rate values to produce an estimate of sensor 
orientation derived fiom the angular rate values; and 

using the estimate of sensor orientation derived from the angular rate values to 
correct for time lag. 

23. A method of tracking the orientation of a sensor, the method comprising: 

a) providing a starting estimate of sensor orientation quaternion; 

b) measuring a magnetic field vector to generate local magnetic field 
vector values; 

c) measuring an acceleration vector to generate local gravity vector 

values; 

d) determining a measurement vector from the local magnetic field vector 
values and the local gravity vector values; 

e) calculating a computed measurement vector fix>m the estimate of 

sensor orientation^ using quaternion mathematics; 
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f) comparing the measurement vector with the computed measurement 
vector to generate an 6x1 error vector that dejBnes a criterion function; and 

g) performing a mathematical operation that results in the minimization 
of the criterion function and outputs a 4x1 quaternion error estimate; 

5 h) integrating the quaternion error estimate; and 

i) normalizing the integrated quaternion error estimate to produce a new 
estimated sensor orientation quaternion; 

j) repeating steps a)-j), wherein the new estimated sensor orientation 
quaternion is used for e), calculating a computed measurement vector. 

10 

24. The method of Claim 23 wherein the operation of g), performing a 
mathematical operation that results in the minimization of the criterion function and 
outputs a 4x1 quaternion error estimate comprises minimizing the criterion function 
witibiout calculating the criterion function. 

15 

25. The method of Claim 23 wherein the operation of g), performing a 
mathematical operation that results in the minimization of the criterion function and 
outputs a 4x1 quaternion eacroi estimate comprises multiplying the 6x1 error vector by 
the function [X'^Xr^X'^. 

20 

26. The method of Claim 23 wherein the operation of g), performing a 
mathematical operation that results in the minimization of the criterion function and 
outputs a 4x1 quaternion error estimate further includes implementing a partial 
correction step to compensate for measurement enor. 

25 

27. The method of Claim 23 wherein the operation of g), performing a 
mathematical operation that results in the minimization of the criterion function and 
outputs a 4x1 quaternion error estimate comprises using a time weighted filtering 
system. 

30 

28. The method of Claim 23 wherein tiie operation of g), performing a 
mathematical operation that results in the minimization of the criterion function and 
outputs a 4x1 quaternion error estimate conq)rises using a Gauss-Newton iteration. 
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29. The method of Claim 26 wherein unplementing the partial correction step to 
compensate for measurement error is supplemented by using a weighted least squares 
regression to emphasize more reliable measurements with respect to less reliable 
measurements. 

30. The method of Claim 23 wherein the operation of g), performing a 
mathematical operation that results in the mininiization of the criterion function and 
outputs a 4x1 quaternion error estimate includes: 

measuring an angular velocity of the sensor to generate an angular rate 
quaternion; 

integrating the angular rate quaternion; 

normalizing the integrated angular rate quaternion to produce an estimate of 
sensor orientation quaternion derived fix>m the angular rate quaternion; and 

using tiie estimate of sensor orientation quaternion derived from the angular 
rate quaternion to correct for time lag. 

31. A sensor apparatus comprising: 

a magnetic field detector configured to measure a magnetic field vector and 
output a local magnetic field vector signal; and 

an acceleration detector configured to detect a local gravitational field vector 
and output a local gravitational field vector signal. 

32. The sensor of Claim 31 , further includes an angular velocity detector 
configured to detect an angular velocity vector of the sensor and output angular 
velocity signal. 

33. The sensor of Claim 32 wherein, the angular rate detector comprises a three- 
axis angular velocity detector; the magnetic field detector comprises a three-axis 
magnetometer; and the acceleration detector comprises a three-axis accelerometer. 

34. The sensor of Claim 31 wherein the sensor includes at least one processor that 
receives and processes the signals from the magnetic field detector and the 
acceleration detector to determine the orientation of the sensor apparatus. 
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35. The sensor of Claim 32 wherein the sensor includes at least one processor that 
recdves and processes the signals &om the magnetic field detector, the acceleration 
detector, and the signal from the angular velocity detector, wherein the at least one 
processor is configured to determine the orientation of the sensor. 

36. A system for tracking the posture and orientation of body, the system 
comprising: 

the body having moimted thereon at least one sensor; 

each sensor including a magnetometer for measuring a magnetic field vector 
and a acceleration detector for measuring a body acceleration vector, and 

at least one processor for receiving input firom the magnetometer and 
acceleration detector and using said input to calculate a local magnetic field vector 
and a local gravity vector and to determine the orientation of the body. 

37. A system as in Claim 36 wherein the at least one processor is configured input 
the body orientation information into a synthetic environment; and 

wherein the system further includes a display for displaying the position and 
orientation of the body with respect to the synthetic environment 

38. A system as in Claim 37 wherein the at least one processor is configured to 
correct for the ofi&et between sensor coordinates and body coordinates. 

39. A system as in Claim 36 wherein each sensor fiuiher includes an angular 
velocity detector for measuring a body angular velocity vector. 

40. A system as in Claim 39 wherein the at least one processing is configured 
input the body orientation information into a synthetic environment; and 

wherein the system further includes a display for displaying the position and 
orientation of the body with respect to the synthetic environment. 

41. A system as in Claim 40 wherein the at least one processor is configured to 
correct for the offset between sensor coordinates and body coordinates. 
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42. A system as in Claim 36 wherein the body comprises an articulated rigid body 
having a plurality of segments interconnected by at least one joint and wherein each 
segment has mounted thereon at least one sensor. 

5 43. A system as in Claim 39 wherein the body comprises an articulated rigid body 
having a pluraUty of segments interconnected by at least one joint and wherein each 
segment has mounted thereon at least one sensor. 

44. A method of determining the direction of a local gravity vector with an 
10 acceleration detector, the method comprising: 

moving the acceleration detector from a start point to an end point over a time 

period; 

taking measuremmts of the total accel^tion vector during the time period; 
weigihted summing the measurements of the total acceleration vector over the 
15 time period; and 

calculating gravity vector values using the weighted sum of the total 
acceleration measurements. 
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